{
 "cells": [
  {
   "cell_type": "markdown",
   "id": "09ebfcde",
   "metadata": {},
   "source": [
    "# 机械臂整机控制例程"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 8,
   "id": "65065134-3702-41d2-9ccf-eefdfc9d1e5d",
   "metadata": {},
   "outputs": [],
   "source": [
    "from six_dof_arm import SixDofArm\n",
    "arm=SixDofArm()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 9,
   "id": "17a3bf97-850d-4047-aabb-ff13ac6dacd0",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.enable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 73,
   "id": "7f65579f-c1f7-43b2-a3a9-4dd9113cfd26",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.disable()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 12,
   "id": "189aa1d5-2547-4bf6-9f93-087c53b18478",
   "metadata": {
    "tags": []
   },
   "outputs": [
    {
     "data": {
      "application/vnd.jupyter.widget-view+json": {
       "model_id": "48ff617621cc4023ad599fe93f6ef8b8",
       "version_major": 2,
       "version_minor": 0
      },
      "text/plain": [
       "interactive(children=(FloatSlider(value=-0.0007963267948964958, description='a1', max=1.5707963267948966, min=…"
      ]
     },
     "metadata": {},
     "output_type": "display_data"
    }
   ],
   "source": [
    "from ipywidgets import interact\n",
    "from math import pi\n",
    "@interact(a1=(-pi/2, pi/2, 0.01), a2=(-pi/2, pi/2, 0.01), a3=(-pi/2, pi/2, 0.01), a4=(-pi/2, pi/2, 0.01), a5=(-pi/2, pi/2, 0.01), a6=(-pi/2, pi/2, 0.01), d=(0,1,1))\n",
    "def calibrate(a1, a2, a3, a4, a5, a6, d=0):\n",
    "    arm.set_motor_pos(0, a1)\n",
    "    arm.set_motor_pos(1, a2)\n",
    "    arm.set_motor_pos(2, a3)\n",
    "    arm.set_motor_pos(3, a4)\n",
    "    arm.set_motor_pos(4, a5)\n",
    "    arm.set_motor_pos(5, a6)\n",
    "    arm.set_nozzel(d)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "7050ed41",
   "metadata": {},
   "source": [
    "#### 初始化机械臂"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 11,
   "id": "9f0619dd",
   "metadata": {
    "tags": []
   },
   "outputs": [],
   "source": [
    "arm.init()"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "497a4be8",
   "metadata": {},
   "source": [
    "#### 机械臂复位"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 5,
   "id": "2265737a",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.home(3)"
   ]
  },
  {
   "cell_type": "markdown",
   "id": "8f7efc59-de2f-4d6a-aa25-30a1ab41b003",
   "metadata": {},
   "source": [
    "#### 抓取"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 72,
   "id": "699b0f02-e587-4db7-9e74-f7c63a0cbd9c",
   "metadata": {},
   "outputs": [
    {
     "name": "stdout",
     "output_type": "stream",
     "text": [
      "551.0 394.0\n"
     ]
    }
   ],
   "source": [
    "import socket\n",
    "import sys\n",
    "import os\n",
    "import time\n",
    "\n",
    "arm_sock = './arm.sock'\n",
    "\n",
    "if os.path.exists(arm_sock):\n",
    "    os.unlink(arm_sock)\n",
    "    \n",
    "sock = socket.socket(socket.AF_UNIX, socket.SOCK_STREAM)\n",
    "sock.bind(arm_sock)\n",
    "sock.listen(5)\n",
    "conn, clientAddr = sock.accept()\n",
    "try:\n",
    "    while True:\n",
    "        data = conn.recv(10)\n",
    "        data = data.decode()\n",
    "        x,y = data.split(',')\n",
    "        x = float(x)\n",
    "        y = float(y)\n",
    "        print(x,y)\n",
    "        z = 0.4652*y + 36.401\n",
    "        z = z / 1000.0\n",
    "        arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.25), 2)\n",
    "        arm.move_to_axis(arm.inverse_kinemetics(-0.128,0.15,z), 1)\n",
    "        arm.set_nozzel(True)\n",
    "        arm.delay(0.5)\n",
    "        arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.2), 1)\n",
    "        arm.move_to_axis(arm.inverse_kinemetics(0.17,0.25,0.12), 1)\n",
    "        arm.move_to_xyz((0.17,0.25,0.12), (0.17,0.1,0.12), dur = 1)\n",
    "        arm.delay(0.5)\n",
    "        arm.set_nozzel(False)\n",
    "        arm.delay(0.5)\n",
    "        arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.2), 1.4)\n",
    "        arm.home(2)\n",
    "        break\n",
    "except Exception as e:\n",
    "    print(e)\n",
    "    pass\n",
    "finally:\n",
    "    conn.close()\n",
    "    sock.close()"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 59,
   "id": "9a60c1d2-0954-4c61-8e11-728f21065d26",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.25), 3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "d9439bd0-ed11-499d-8fc8-b17ff37c1618",
   "metadata": {},
   "outputs": [],
   "source": [
    "-0.13,0.22,533,398\n",
    "-0.13,0.225,537,411\n",
    "-0.127,0.248,541,449\n",
    "-0.13,0.19,536,327"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 60,
   "id": "f373fb3c-8071-4be4-bb68-a59c050714cc",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_axis(arm.inverse_kinemetics(-0.128,0.15,0.19), 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 37,
   "id": "67050325-b448-41ba-afd2-d151ab7670e5",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.set_nozzel(True)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 38,
   "id": "9e748da8-f5c4-499b-ba7e-6cc46dade27f",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.2), 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 39,
   "id": "a189ce5c-3982-47bb-894c-5362da3bdd16",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_axis(arm.inverse_kinemetics(0.17,0.25,0.12), 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 40,
   "id": "0ca70b78-5d04-4704-b8dc-25873d2a813a",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_xyz((0.17,0.25,0.12), (0.17,0.1,0.12), dur = 1)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 41,
   "id": "aba711ac-9e41-42cd-a7e5-b778a70c06aa",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.set_nozzel(False)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": 61,
   "id": "a92eb6fe-0e1b-4df9-9733-519d83e3ee17",
   "metadata": {},
   "outputs": [],
   "source": [
    "arm.move_to_axis(arm.inverse_kinemetics(-0.1,0.25,0.2), 2)\n",
    "arm.home(3)"
   ]
  },
  {
   "cell_type": "code",
   "execution_count": null,
   "id": "cc18eaa6-9881-4347-a5ae-4f0039824cc9",
   "metadata": {},
   "outputs": [],
   "source": []
  }
 ],
 "metadata": {
  "kernelspec": {
   "display_name": "Python 3",
   "language": "python",
   "name": "python3"
  },
  "language_info": {
   "codemirror_mode": {
    "name": "ipython",
    "version": 3
   },
   "file_extension": ".py",
   "mimetype": "text/x-python",
   "name": "python",
   "nbconvert_exporter": "python",
   "pygments_lexer": "ipython3",
   "version": "3.6.9"
  }
 },
 "nbformat": 4,
 "nbformat_minor": 5
}
